#include <SCoop.h>
#include <LedControl.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt;                                   //微分时间
 
int16_t ax, ay, az, gx, gy, gz;             //加速度计陀螺仪原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;    //角度变量
long axo = 0, ayo = 0, azo = 0;             //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;             //陀螺仪偏移量

float GyroRatio = 131.0;                    //陀螺仪比例系数
float AcceRatio = 16384.0;


float accx, accy, accz, gyrox, gyroy, gyroz;

//-------led参数

const int DIN = 7;
const int CLK = 9;
const int CS = 8;
LedControl led = LedControl(DIN, CLK, CS , 4);

byte zuo1[][1]=
  { 0x99,0xCC,0x66,0x33,0x33,0x66,0xCC,0x99,
0x99,0xCC,0x66,0x33,0x33,0x66,0xCC,0x99,
0x99,0xCC,0x66,0x33,0x33,0x66,0xCC,0x99,
0x99,0xCC,0x66,0x33,0x33,0x66,0xCC,0x99,
   };

byte zuo2[][1]=
  { 0xCC,0x66,0x33,0x99,0x99,0x33,0x66,0xCC,
0xCC,0x66,0x33,0x99,0x99,0x33,0x66,0xCC,
0xCC,0x66,0x33,0x99,0x99,0x33,0x66,0xCC,
0xCC,0x66,0x33,0x99,0x99,0x33,0x66,0xCC,
    };

byte zuo3[][1]=
  { 0x66,0x33,0x99,0xCC,0xCC,0x99,0x33,0x66,
0x66,0x33,0x99,0xCC,0xCC,0x99,0x33,0x66,
0x66,0x33,0x99,0xCC,0xCC,0x99,0x33,0x66,
0x66,0x33,0x19,0xCC,0xCC,0x19,0x33,0x66,
    };
    
byte zuo4[][1]=
  { 0x33,0x99,0xCC,0x66,0x66,0xCC,0x99,0x33,
0x33,0x99,0xCC,0x66,0x66,0xCC,0x99,0x33,
0x33,0x99,0xCC,0x66,0x66,0xCC,0x99,0x33,
0x33,0x99,0xCC,0x66,0x66,0xCC,0x99,0x33,
    };


byte you1[][1]=
  { 0x33,0x66,0xCC,0x99,0x99,0xCC,0x66,0x33,
0x33,0x66,0xCC,0x99,0x99,0xCC,0x66,0x33,
0x33,0x66,0xCC,0x99,0x99,0xCC,0x66,0x33,
0x33,0x66,0xCC,0x99,0x99,0xCC,0x66,0x33,
    };

byte you2[][1]=
  { 0x66,0xCC,0x98,0x33,0x33,0x98,0xCC,0x66,
0x66,0xCC,0x99,0x33,0x33,0x99,0xCC,0x66,
0x66,0xCC,0x99,0x33,0x33,0x99,0xCC,0x66,
0x66,0xCC,0x99,0x33,0x33,0x99,0xCC,0x66,
    };

byte you3[][1]=
  { 0xCC,0x99,0x33,0x66,0x66,0x33,0x99,0xCC,
0xCC,0x99,0x33,0x66,0x66,0x33,0x99,0xCC,
0xCC,0x99,0x33,0x66,0x66,0x33,0x99,0xCC,
0xCC,0x99,0x33,0x66,0x66,0x33,0x99,0xCC,

    };
byte you4[][1]=
  { 0x99,0x33,0x66,0xCC,0xCC,0x66,0x33,0x99,
0x99,0x33,0x66,0xCC,0xCC,0x66,0x33,0x99,
0x99,0x33,0x66,0xCC,0xCC,0x66,0x33,0x99,
0x99,0x33,0x66,0xCC,0xCC,0x66,0x33,0x99,

    };

byte shache1[][1]=
  { 0x3,0x1,0xFC,0x44,0x34,0x9C,0x4D,0x25,
0x20,0x40,0x81,0x0,0x1,0x0,0x0,0x0,
0x4,0x2,0x81,0x0,0x80,0x0,0x0,0x0,
0xC0,0x80,0x3F,0x22,0x2C,0x39,0xB2,0xA4,

    };


byte kong[][1]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,};





byte eye0[][1]={0x0,0x7C,0xFE,0xFE,0xCE,0xCE,0x7C,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x7F,0x67,0x67,0x3E,0x0,
};

byte eye1[][1]={0x0,0x7C,0xFE,0xFE,0xE6,0xE6,0x7C,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x7F,0x73,0x73,0x3E,0x0,
};
byte eye2[][1]={0x0,0x7C,0xFE,0xFE,0xF2,0xF2,0x7C,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x7F,0x79,0x79,0x3E,0x0,
};
byte eye3[][1]={0x0,0x7C,0xFE,0xF2,0xF2,0x7C,0x0,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x79,0x79,0x3E,0x0,0x0,

};
byte eye4[][1]={0x0,0x7C,0xF2,0xF2,0x7C,0x0,0x0,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x79,0x79,0x3E,0x0,0x0,0x0,
};

byte eye5[][1]={0x0,0x7C,0xE6,0x7C,0x0,0x0,0x0,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x73,0x3E,0x0,0x0,0x0,0x0,
};

byte eye6[][1]={0x0,0x7C,0xCE,0xCE,0x7C,0x0,0x0,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x67,0x67,0x3E,0x0,0x0,0x0,

};

byte eye7[][1]={0x0,0x7C,0xFE,0xCE,0xCE,0x7C,0x0,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x67,0x67,0x3E,0x0,0x0,

};

byte eye8[][1]={0x0,0x7C,0xFE,0xCE,0xCE,0xFE,0x7C,0x0,
0x0,0xE0,0x10,0x0,0x0,0x0,0x0,0x0,
0x0,0x7,0x8,0x0,0x0,0x0,0x0,0x0,
0x0,0x3E,0x7F,0x67,0x67,0x7F,0x3E,0x0,
};


//------


 
defineTask(TaskTest1);//创建子线程1
defineTask(TaskTest2);//创建子线程2
void TaskTest1::setup()//线程1设定
{
    Serial.begin(115200);
     
    accelgyro.initialize();                 //初始化
          unsigned short times = 200;             //采样次数
    for(int i=0;i<times;i++)
    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
        axo += ax; ayo += ay; azo += az;      //采样和
        gxo += gx; gyo += gy; gzo += gz;
    
    }
    
    axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}
void TaskTest1::loop()//线程1循环
{
    unsigned long now = millis();             //当前时间(ms)
    dt = (now - lastTime) / 1000.0;           //微分时间(s)
    lastTime = now;                           //上一次采样时间(ms)
 
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
 

    accx = ax / AcceRatio;              //x轴加速度
 //   accy = ay / AcceRatio;              //y轴加速度
 //   accz = az / AcceRatio;              //z轴加速度
 //   gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
 //   gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
    gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
    

    Serial.print(accx);Serial.print(",");
//    Serial.print(accy);Serial.print(",");
//    Serial.print(accz);Serial.print(",");
//    Serial.print(gyrox);Serial.print(",");
//    Serial.print(gyroy);Serial.print(",");
    Serial.print(gyroz);Serial.println();
}

void showyou();
void showzuo();
void shache();
void showWordByPtr(LedControl led, byte (*word)[1]);
void showeye(int x);
int y=0;
 
void TaskTest2::setup()//线程2设定
{
for (int i = 0; i < 4; ++i)
      {
        led.shutdown(i, false);
        led.clearDisplay(i);
        led.setIntensity(i,2);
      }
      
}
void TaskTest2::loop()//线程2循环
{
    if(gyroz > 0.2 and gyroz <1 ){
 //   Serial.println("右");
    showyou();
    showyou();
    showyou();

    
    }

if(gyroz < -0.2 and gyroz >-1){
 //   Serial.println("左");
    showzuo();
    showzuo();
    showzuo();

    }

    if(accx > 0.8 and accx <1.5 ){
     for(int i=0;i<10;i++){
      shache();
      }

    }
   if(accx < -0.8 and accx >-1.5 ){
     for(int i=0;i<10;i++){
      shache();
      }

    }
    
   showeye(y);
   sleep(100);
   y++;
   if(y>50)y=0;
   
}
void setup() {
mySCoop.start();
}
 
void loop()
{
  yield();
}

void showWordByPtr(LedControl led, byte (*word)[1])
{ int temp1=0;
  for (int i = 0; i < 4; ++i)
  {
    
    for (int j = 0; j < 8; ++j)
      {
        led.setRow(i, j, *(*(word + j)+temp1));
      }
      temp1 +=8;
  }
  
}

void showzuo(){
      showWordByPtr(led, zuo1);
      sleep(150);
      showWordByPtr(led, zuo2);
      sleep(150);
      showWordByPtr(led, zuo3);
      sleep(150);
      showWordByPtr(led, zuo4);
      sleep(150);
      
}

void showyou(){
      showWordByPtr(led, you1);
      sleep(150);
      showWordByPtr(led, you2);
      sleep(150);
      showWordByPtr(led, you3);
      sleep(150);
      showWordByPtr(led, you4);
      sleep(150);
      
}

void shache(){
      showWordByPtr(led, shache1);
      sleep(50);
      showWordByPtr(led, kong);
      sleep(50);
}

void showeye(int x){
  switch (x){
    case 0 :showWordByPtr(led, eye0);break;
    case 1 :showWordByPtr(led, eye1);break;
    case 2 :showWordByPtr(led, eye2);break;
    case 3 :showWordByPtr(led, eye3);break;
    case 4 :showWordByPtr(led, eye4);break;
    case 5 :showWordByPtr(led, eye5);break;
    case 6 :showWordByPtr(led, eye6);break;
    case 7 :showWordByPtr(led, eye7);break;
    case 8 :showWordByPtr(led, eye8);break;
    default:showWordByPtr(led, eye0);
    
  }
  
}
